#include "lidar/classify/lidar_noise_filter.h"

namespace avos {
namespace perception {

LidarNoiseFilter::LidarNoiseFilter(double stamped_buffer_length, double latency)
    : time_latency_(latency),
      grid_map_(7.0, 5.0, 0.2),
      cells_with_pose_list_(ros::Duration(stamped_buffer_length)){};

void LidarNoiseFilter::Filter(ros::Time data_time,
                              DummyVector<LidarObject>& lidar_objects,
                              Rigid3<float> current_pose) {
  // reset
  accumulate_local_cells_.clear();

  // TIPS: check cloud queue, time roll back
  if (!cells_with_pose_list_.empty()) {
    ros::Time latest_cloud_time = cells_with_pose_list_.latest_time();
    if (data_time < latest_cloud_time) {
      ROS_ERROR_STREAM("[Lidar Noise Filter] input time roll back");
      cells_with_pose_list_.Clear();
    }
  }

  // get cloud lidar noise/tiny objects cells
  const uint8_t tiny_type_id =
      static_cast<uint8_t>(LidarObjectGroup::ObjectType::TINY);
  const uint8_t noise_type_id =
      static_cast<uint8_t>(LidarObjectGroup::ObjectType::NOISE);
  const uint8_t short_low_id =
      static_cast<uint8_t>(LidarObjectGroup::ObjectType::SHORT_LOW);

  // clear grid map
  grid_map_.reset();
  auto iter_begin = cells_with_pose_list_.rbegin();
  auto iter_end = cells_with_pose_list_.rend();
  Rigid3<float> base_pose = current_pose;
  Rigid3<float> base_pose_inverse = base_pose.inverse();
  //
  while (iter_begin != iter_end) {
    ros::Time latest_cloud_time = cells_with_pose_list_.latest_time();
    if ((latest_cloud_time - iter_begin->stamp_).toSec() > time_latency_) {
      Rigid3<float> local_pose = base_pose_inverse * iter_begin->data_.pose;
      for (const auto& cell : iter_begin->data_.cells) {
        Eigen::Vector3f trans_cell = local_pose * cell;
        accumulate_local_cells_.push_back(std::move(trans_cell));
        grid_map_.set(trans_cell[0], trans_cell[1]);
      }
    }

    iter_begin++;
  }

  // collect TINY & SHORT LOW cells
  DummyVector<Eigen::Vector3f> sensitive_cells;
  for (const LidarObject& object : lidar_objects) {
    // this is not include noise type. just recorver the object type
    // whitch misclassification to noise type
    // eg. TINY->TINY->[NOISE]  <recovered>
    // eg. NOISE->NOISE->[NOISE] <not recovered>
    if (object.type == tiny_type_id || object.type == short_low_id
        /*||object.type == noise_type_id*/) {
      for (const LidarCell& cell : object.cells) {
        Eigen::Vector3f sensi_cell(cell.position[0], cell.position[1], 0.0);
        sensitive_cells.push_back(std::move(sensi_cell));
      }
    }
  }

  // check objects history occupancy
  for (LidarObject& object : lidar_objects) {
    if (object.type == noise_type_id) {
      for (const LidarCell& cell : object.cells) {
        if (cell.position[0] < 10 && cell.position[0] > 0 &&
            cell.position[1] < 1 && cell.position[1] > -4)
          // there are history occupancy objects in grid view
          // so recover the tiny objects
          if (grid_map_.get(cell.position[0], cell.position[1]) >= 1) {
            object.type = tiny_type_id;
            // // insert this object cells
            // for (const LidarCell& cell : object.cells) {
            //   Eigen::Vector3f sensi_cell(cell.position[0], cell.position[1],
            //   0.0); sensitive_cells.push_back(std::move(sensi_cell));
            // }
            break;
          }
      }
    }
  }

  // push current cells list into cells array
  StampedStorage<CellsWithPose> stamped_cells_with_pose(
      data_time, CellsWithPose(sensitive_cells, base_pose));

  // finally push latest sensitive cells input cell history list
  cells_with_pose_list_.Push(std::move(stamped_cells_with_pose));
}

void LidarNoiseFilter::Visualize(pcl::PointCloud<pcl::PointXYZ>& cells_cloud) {
  // for (auto& cell : accumulate_local_cells_) {
  //   pcl::PointXYZ point_xyz;
  //   point_xyz.x = cell[0];
  //   point_xyz.y = cell[1];
  //   point_xyz.z = -0.1;
  //   cells_cloud.points.push_back(point_xyz);
  // }

  cells_cloud = grid_map_.visualize();
}
}  // namespace perception
}  // namespace avos